#include "CPFLocalizer.h"

/** All basic initializations are taken care of by the constructor of the base class **/
CPFLocalizer::CPFLocalizer(ros::NodeHandle &n) :
	Localizer(n), cpfsm(n), pd(n), config("config/general.cfg") {
	readOdomInvoked = 0;
	Nr = atoi(config.getConfiguration("robot.nPosesForGaussian").c_str());
	Ny = atoi(config.getConfiguration("human.nPosesForGaussian").c_str());
	subm = n.subscribe("manpose", 10, &CPFLocalizer::manPoseCallback, this);
	trackService = n.advertiseService("stalker_track", &CPFLocalizer::Track, this);
	/**
	 *  M starts with 0, becomes 1 when a track command is given by the master
	 *  and then changes dynamically by model selection
	 */
	M = 0;

	for (int i = 0; i < Nr; i++)
		cpf.robotEstimateOfPersonsConfigurations.push_back(RobotSet(Ny, Configuration()));

	cpfsm.setObstacleCostMap(obstaclecost.cost);
	pd.setObstacleCostMap(obstaclecost.cost);
}

bool CPFLocalizer::Track(cpf::TrackPos::Request &req, cpf::TrackPos::Response &res) {
	std::cout << "TC " << poseOfTarget.first.first << " " << poseOfTarget.first.second << "\n";
	cm.PlotPoses(poseOfTarget.first.second*10,poseOfTarget.first.first*10,cost_marker_pub,0.1f);
	res.personX = poseOfTarget.first.first;
	res.personY = poseOfTarget.first.second;
	res.confidence = 0.90f;
	return true;
}

void CPFLocalizer::manPoseCallback(const geometry_msgs::PoseStamped &msg) {
	std::cout << "MPCB ";
	/** The robot pose must have been provided first **/
	assert(initialEstimateProvided);
	PoseArray personOfInterest;
	M = 1;
	PoseCreator.getInitialHumanPoses(msg, personOfInterest, map, config);
	/**
	 * At the end of this loop, there are Ny configurations PER robot pose,
	 * and each configuration has exactly one person <-- M == 1
	 * This  is the general method to add a new person to world.
	 * 		# call getInitialHumanPoses
	 * 		# push each of the Ny * Nr particles arbitly into a configuration,
	 * 		# assert that each configuration is of size M
	 */
	for (int i = 0; i < Nr; i++)
		for (int j = 0; j < Ny; j++) {
			cpf.robotEstimateOfPersonsConfigurations[i][j].clear();
			cpf.robotEstimateOfPersonsConfigurations[i][j].push_back(personOfInterest[j]);
		}
	poseOfTarget = DDD(DD(msg.pose.position.x, msg.pose.position.y),0);
}

/** Parent readOdom overridden  to not call localize method often
 *	readOdom is called ten times a second. We invoke the CPF::localize method
 *	once every 10 invocations of readOdom i.e., once every second instead of calling it
 *	once there is significant robot motion because people may keep moving
 ***/
void CPFLocalizer::readOdom(nav_msgs::Odometry msg) {
	readOdomInvoked++;
	OdomReading current(msg);

	if (last.x == 0 && last.y == 0 && last.theta == 0)
		last = current;
	this->localize(last,current);
}


void CPFLocalizer::localize(OdomReading &lst, OdomReading &current) {
	if (!initialEstimateProvided)	return;
	bool changed = compareCurrentOdomReadingToNew(last, current);
//	if (M <= 1) {
//		/** We call the normal particle filter only if there is significant change in the odom readings **/
//		if (changed) {
//			Localizer::localize(last, current);
//			last = current;
//		}
//		return;
//	}

	if(readOdomInvoked <= 5)
			return;
	readOdomInvoked = 0;
	ROS_INFO("Setting Odom");
	om.setOdomMove(last, current);
	ROS_INFO("Resampling");
	ROS_INFO("ReM: %d", M);
	ROS_INFO("[CPL] size of means: %d", cpf.means.size());
	bool rotateMajor = tooMuchRotation(last,current);
	if(rotateMajor)
		cpf.reSample(pa, w, mm, cpfsm, map, config, om, map_to_odom, poseOfTarget, M, sm);
	else
		map_to_odom = cpf.reSample(pa, w, mm, cpfsm, map, config, om, map_to_odom, poseOfTarget, M, sm);
//		cpf.reSample(pa, w, mm, sm, map, config, om, map_to_odom, poseOfTarget, M);

	ROS_INFO("Estimating #people");
	// figure out how many people are there based on BRP
	ROS_INFO("EpM: %d", M);
	int currentM = M;
	STLPose newPersonPose;
	pd.estimateNumberOfPersons(	pa[cpf.indexOfBestRobotPose],
								cpf.robotEstimateOfPersonsConfigurations[cpf.indexOfBestRobotPose][cpf.indexOfBestPersonConfiguration],
								cpf.means, cpf.stds, config, M, newPersonPose, map
								);
	ROS_INFO("After EpM: %d, %d", M, currentM);

	// if a new person has arrived into the scene
	if (currentM+1 == M)	{

		PoseArray personOfInterest;
		// create a pose message for where we think (s)he is.
		double x = newPersonPose.first.first, y = newPersonPose.first.second, theta = newPersonPose.second;
		ROS_INFO("Creating new human poses at %0.2f %0.2f", x, y);
		PoseCreator.getInitialHumanPoses(x, y, theta, personOfInterest, map, config);
		ROS_INFO("Filling in entries");
		for (int i = 0; i < Nr; i++)	{
			for (int j = 0; j < Ny; j++) {
				cpf.robotEstimateOfPersonsConfigurations[i][j].push_back(personOfInterest[j]);
			}
		}

		if (M == 1)	{
			double x = newPersonPose.first.first, y = newPersonPose.first.second, theta = newPersonPose.second;
			poseOfTarget = DDD(DD(x, y), theta);
		}

		cpf.findMeansAndStdsOfPersons(M);
		cout << "Size of means: " << cpf.means.size() << endl;

	}


	// remove persons who are too spread out
	// i.e. we are no longer sure where they are
	ROS_INFO("Going to reduce persons");
	vector<RobotSet> reducedRobotEstimateOfPersonsConfigurations;
	for (int i = 0; i < Nr; ++i){
		reducedRobotEstimateOfPersonsConfigurations.push_back(RobotSet());
		for (int j = 0; j < Ny; ++j)	{
			reducedRobotEstimateOfPersonsConfigurations[i].push_back(Configuration());
			for (int k = 0; k < M; ++k){
//					ROS_INFO("Checking %d %d %d", i, j, k);
				MyPose person = cpf.robotEstimateOfPersonsConfigurations[i][j][k];
				if (cpf.stds[k].first < 0.5 && cpf.stds[k].second < 0.5)	{
					reducedRobotEstimateOfPersonsConfigurations[i][j].push_back(person);
					if (i == 0 && j == 0)	{
						cout << "Good std: "  << cpf.stds[k].first << ", " <<cpf.stds[k].second << endl;
					}
				}
//				else	{
//					cout << "==============\nThrowing out person " << k << " because std is [" << cpf.stds[k].first << ", " << cpf.stds[k].second << "]\n================\n" << endl;
//				}
			}
		}
	}
	cpf.robotEstimateOfPersonsConfigurations = reducedRobotEstimateOfPersonsConfigurations;
	M = cpf.robotEstimateOfPersonsConfigurations[0][0].size();

	cout << "New size of configs is " << cpf.robotEstimateOfPersonsConfigurations[0][0].size() << endl;





	last = current;

	cm.PlotParticles(cpf.robotEstimateOfPersonsConfigurations[cpf.indexOfBestRobotPose], cost_marker_pub);
	paviz.publishPoseArray(pa);

}
